#include <iostream>
#include <localization/test/localization_test.h>
#include <reactive_behavior/test/reactive_behavior_test.h>
#include <core/test/planner_reactive_test.h>
#include <core/execution.h>
// TODO include other tests.

using std::cout;
using std::cin;
using std::endl;

int main() {
  int option;
  cout << "Enter an option:" << endl;
  cout << "0: Run delivery robot!" << endl;
  cout << "1: Run reactive behavior test with faked localization estimation." << endl;
  cout << "2: Run path planning test with faked localization estimation and faked obstacle avoidance." << endl;
  cout << "3: Run path planning and reactive behavior test with faked localization estimation." << endl;
  cout << "4: Run localization estimation test with faked lamp detection." << endl;
  cout << ">>> ";
  cin >> option;
  switch (option) {
    case 0:
      ExecuteDeliveryRobot();
      break;
    case 1:
      TESTReactiveBehavior();
      break;
    case 2:
      // TODO implement TESTPlanner();
      break;
    case 3:
      // TODO fix TESTPlannerReactive();
    case 4:
      TESTLocalization();
      break;
    default:
      cout << "Invalid option. Ending execution." << endl;
  }
  return 0;
}
